#include "sensors_layer.h"

namespace costmap_2d{

SensorLayer::SensorLayer(boost::shared_ptr<Costmap> realtime_costmap_ptr, tf2_ros::Buffer* tf_buffer)
    :realtimeCostmapPtr_(realtime_costmap_ptr), tfBuffer_(tf_buffer){
    keepTime_ = ros::Duration(0.0);

    debugOriginPointCloudPub_ = nh_.advertise<sensor_msgs::PointCloud2>("/debug_raw_pointcloud", 1);
    debugTransformedPointCloudPub_ = nh_.advertise<sensor_msgs::PointCloud2>("/debug_transformed_pointcloud", 1);
}

SensorLayer::~SensorLayer(){

}


// 更新代价地图
void SensorLayer::updateCostmap(bool is_local){
    obstacleCells_.clear();
    for(auto iter = pointCloudDeque_.begin(); iter != pointCloudDeque_.end(); ++iter){
        // const sensor_msgs::PointCloud2& temp_cloud = iter->cloud;
        geometry_msgs::TransformStamped sensor_pose = iter->sensor_pose;
        sensor_msgs::PointCloud2 cloud = iter->cloud;

        sensor_msgs::PointCloud2ConstIterator<float> iter_x(cloud, "x");
        sensor_msgs::PointCloud2ConstIterator<float> iter_y(cloud, "y");
        sensor_msgs::PointCloud2ConstIterator<float> iter_z(cloud, "z");
        tf2::Transform tf2_transform_front;
        tf2::convert(pointCloudDeque_.front().sensor_pose.transform, tf2_transform_front);
        tf2::Transform tf2_transform;
        tf2::convert(sensor_pose.transform, tf2_transform);
        tf2::Transform tf_transform = tf2_transform_front.inverse() * tf2_transform;

        // const tf2::Vector3& translation = tf_transform.getOrigin();
        // std::cout << "tf_transform Translation: ["
        //       << translation.x() << ", "
        //       << translation.y() << ", "
        //       << translation.z() << "]" << std::endl;


        for (; iter_x !=iter_x.end(); ++iter_x, ++iter_y, ++iter_z){
            int mx, my;

            double px = *iter_x, py = *iter_y, pz = *iter_z;
            // realtimeCostmapPtr_->worldToMap(px, py, mx, my);

            tf2::Vector3 point_in(px, py, pz);
            tf2::Vector3 point_out = tf_transform * point_in;

            // // tf2::Vector3 point_out = tf_transform * point_in;
            realtimeCostmapPtr_->worldToMap(point_out.x(), point_out.y(), mx, my);
            if(!realtimeCostmapPtr_->isValid(mx, my)){
                continue;
            }
            int index = realtimeCostmapPtr_->mapToIndex(mx, my);

            if(index < 0 || index >= realtimeCostmapPtr_->costMap.size()) {
                continue; 
            }
            realtimeCostmapPtr_->costMap[index] = costmap_2d::LETHAL_OBSTACLE;

            GridCell grid_cell;
            grid_cell.index = index;
            grid_cell.x = mx;
            grid_cell.y = my;
            grid_cell.sx = mx;
            grid_cell.sy = my;
            grid_cell.cost = costmap_2d::LETHAL_OBSTACLE;
            obstacleCells_[index] = grid_cell;
        }
    }

    purgeDeque();
}

// 添加二维激光雷达传感器数据
void SensorLayer::addLidar2DScanData(sensor_msgs::LaserScan::Ptr msg_ptr){
    SensorData sensor_data;
    sensor_data.cloud.header = msg_ptr->header;
    sensor_data.cloud.header.frame_id = realtimeCostmapPtr_->frameId;

    try{
        geometry_msgs::TransformStamped transformStamped = tfBuffer_->lookupTransform(
            sensor_data.cloud.header.frame_id, msg_ptr->header.frame_id, msg_ptr->header.stamp);

        // 激光数据转换为点云
        sensor_msgs::PointCloud2 temp_cloud;
        temp_cloud.header = msg_ptr->header;
        projector_.projectLaser(*msg_ptr, temp_cloud);

        // 应用变换，将点云转换到目标坐标系
        tf2::doTransform(temp_cloud, sensor_data.cloud, transformStamped);   

        transformStamped = tfBuffer_->lookupTransform("map", "base_link", msg_ptr->header.stamp);
        sensor_data.sensor_pose = transformStamped;
        // const geometry_msgs::Vector3& translation = transformStamped.transform.translation;
        // std::cout << "  Translation: [" 
        //         << translation.x << ", " 
        //         << translation.y << ", " 
        //         << translation.z << "]" << std::endl;

        if(debugOriginPointCloudPub_.getNumSubscribers() > 0){
            debugOriginPointCloudPub_.publish(temp_cloud);  //调试用
        }
        if(debugTransformedPointCloudPub_.getNumSubscribers() > 0){
            debugTransformedPointCloudPub_.publish(sensor_data.cloud);  //调试用 
        }
    }
    catch(tf2::TransformException &ex){
        ROS_WARN("High fidelity enabled, but TF returned a transform exception to frame %s: %s", 
            sensor_data.cloud.header.frame_id.c_str(), ex.what());
        return; 
    }
    catch (std::runtime_error &ex){
        ROS_WARN("transformLaserScanToPointCloud error, it seems the message from laser sensor is malformed. Ignore this laser scan. what(): %s", ex.what());
        return; 
    }

    pointCloudDeque_.emplace_front(sensor_data);
    purgeDeque();
}

// 添加点云数据
void SensorLayer::addPointCloud2Data(sensor_msgs::PointCloud2::Ptr msg_ptr){
    SensorData sensor_data;
    sensor_data.cloud.header = msg_ptr->header;
    sensor_data.cloud.header.frame_id = realtimeCostmapPtr_->frameId;
    try{
        geometry_msgs::TransformStamped transformStamped = tfBuffer_->lookupTransform(
            realtimeCostmapPtr_->frameId, msg_ptr->header.frame_id, msg_ptr->header.stamp);   

        tf2::doTransform(*msg_ptr, sensor_data.cloud, transformStamped); 

        sensor_data.sensor_pose = transformStamped;   
    }   
    catch(tf2::TransformException &ex){
        ROS_WARN("High fidelity enabled, but TF returned a transform exception to frame %s: %s", 
            realtimeCostmapPtr_->frameId.c_str(), ex.what());
        return; 
    }  
    catch (std::runtime_error &ex){
        ROS_WARN("transformLaserScanToPointCloud error, it seems the message from laser sensor is malformed. Ignore this laser scan. what(): %s", ex.what());
        return; //ignore this message
    }

    pointCloudDeque_.emplace_front(sensor_data);
    purgeDeque();
}

// 传感器队列裁剪
void SensorLayer::purgeDeque(){
    if(!pointCloudDeque_.empty()){
        auto iter = pointCloudDeque_.begin();
        if(keepTime_ == ros::Duration(0.0)){
            pointCloudDeque_.erase(++iter, pointCloudDeque_.end());
        }
        else{
            for(iter = pointCloudDeque_.begin(); iter != pointCloudDeque_.end(); ++iter){
                if((ros::Time::now() - iter->cloud.header.stamp) > keepTime_){
                    pointCloudDeque_.erase(iter, pointCloudDeque_.end());
                    return;
                }
            }
        }
    }
}

// 清空代价地图
void SensorLayer::clearCostmap(){
    obstacleCells_.clear();
    originCostmapPtr_ = nullptr;
}

}; //costmap_2d